#ifndef _ADGUSTSPEED__H
#define _ADGUSTSPEED__H
#include <stdint.h>
#include <drv_pwmMotors.h>
#define Pan_RtoS_Shift PanMotorPoles*0.000191747//PanMotorPoles*PI/16384

#define Encode_Array_Size	9

#define maxTargetDelta (40.96/16.0)
#define minTargetDelta (0.1138/6.0)

//#define Q_angle 0.0//0.005178//0.001*(16384/360)*(16384/360)*2.5e-3
#define Q_delta 0.03	//0.015534//.003*(16384/360)*(16384/360)*2.5e-3
#define R_measure 240.0 //62.1378//.03*(16384/360)*(16384/360)
#define alpha2 1.0		//0.95

/*********************************************
 * 电机转子位置计算结构体
 * ******************************************/
typedef struct 
{
	uint8_t           index;                                //指针位置
	uint8_t           size;                                 //长度
	uint32_t          encode_arrray[Encode_Array_Size];     //数据缓存
	float             rotor;                                //电机转子位置
}Rotor_Pos_t;

/**********************************************
 * 领跑算法调速结构体定义
 * *******************************************/
typedef struct 
{
	float err;					            //当前误差
	float err_last;				            //上次误差
	float err_last_next;			        //上上次误差
	float Ki;
	float Kp;
	float Kd;
}Lead_speed_t;
/**********************************************
混合调速算法结构体
**********************************************/
typedef struct 
{
	float                angle;                  
	float                delta;
	float                P[2][2];
	float                Ki;
	float                kp;
	float                TargetDeltaTab[65];
	float                Target_Delta;
	float                TargetPos;
	bool                 init_status;
	Lead_speed_t         Lead_speed;                       //领跑调速
	Rotor_Pos_t          rotor;  
	ZY_motor_drive_t     *motor_drive;
}mixer_speed_s;
void Mixer_Init(mixer_speed_s *handle);
bool getRotor(mixer_speed_s *handle, uint32_t value);
float mixer_Speed_fun(mixer_speed_s *handle);
void Motor_Speed(ZY_FOC_Move_t *speed,mixer_speed_s *mixer,ZY_Control_t target);
void Init_MotorSpeed(ZY_FOC_Move_t *speed,mixer_speed_s *mixer);
void set_TargetSpeed(mixer_speed_s *handle, uint8_t speed);    //设置目标速度
#endif
